
/// animation variables !
unsigned long cur_anim_time; 
unsigned long anim_time;
int anim1_head[N_HEAD];
int anim2_head[N_HEAD];
int anim1_right_arm[N_ARM];
int anim2_right_arm[N_ARM];
int anim1_left_arm[N_ARM];
int anim2_left_arm[N_ARM];
int anim1_right_leg[N_LEG];
int anim2_right_leg[N_LEG];
int anim1_left_leg[N_LEG];
int anim2_left_leg[N_LEG];

void update_servos()
{
  for(int i=0; i<N_HEAD; i++)
  {
    servo_head[i].write(pos_head[i]);
  }

  for(int i=0; i<N_ARM; i++)
  {
    servo_left_arm[i].write(pos_left_arm[i]);
    servo_right_arm[i].write(pos_right_arm[i]);
  }

  for(int i=0; i<N_LEG; i++)
  {
    servo_left_leg[i].write(pos_left_leg[i]);
    servo_right_leg[i].write(pos_right_leg[i]);
  }
}

int anim_step_pos(int a, int b, float s)
{
  if(a==b) return a;

  float r = ((float)a*s) + ((float)b*(1.0f-s));
  return (int)r;
}

boolean do_anim(unsigned long slicetime)
{
  cur_anim_time += slicetime;

  // first step
  if(cur_anim_time==0)
  {
    for(int i=0; i<N_HEAD; i++)
    {
      pos_head[i] = anim1_head[i];
    }

    for(int i=0; i<N_ARM; i++)
    {
      pos_left_arm[i] = anim1_left_arm[i];
      pos_right_arm[i] = anim1_right_arm[i];
    }

    for(int i=0; i<N_LEG; i++)
    {
      pos_left_leg[i] = anim1_left_leg[i];
      pos_right_leg[i] = anim1_right_leg[i];
    }

    return false;
  }
  // end anim
  if(cur_anim_time>anim_time)
  {
    for(int i=0; i<N_HEAD; i++)
    {
      pos_head[i] = anim2_head[i];
    }

    for(int i=0; i<N_ARM; i++)
    {
      pos_left_arm[i] = anim2_left_arm[i];
      pos_right_arm[i] = anim2_right_arm[i];
    }

    for(int i=0; i<N_LEG; i++)
    {
      pos_left_leg[i] = anim2_left_leg[i];
      pos_right_leg[i] = anim2_right_leg[i];
    }

    return true;
  }
  else
  {
    float s = (float)(anim_time-cur_anim_time)/(float)anim_time;

    for(int i=0; i<N_HEAD; i++)
    {
      pos_head[i] = anim_step_pos(anim1_head[i], anim2_head[i], s);
    }

    for(int i=0; i<N_ARM; i++)
    {
      pos_left_arm[i] = anim_step_pos(anim1_left_arm[i], anim2_left_arm[i], s);
      pos_right_arm[i] = anim_step_pos(anim1_right_arm[i], anim2_right_arm[i], s);
    }

    for(int i=0; i<N_LEG; i++)
    {
      pos_left_leg[i] = anim_step_pos(anim1_left_leg[i], anim2_left_leg[i], s);
      pos_right_leg[i] = anim_step_pos(anim1_right_leg[i], anim2_right_leg[i], s);
    }

    delay(slicetime);
    return false;
  }
}

void set_animation_time(unsigned long ms)
{
  cur_anim_time = ms;
}

void set_animation_step(unsigned long ms, int what1, int what2, int time)
{
  anim_time = ms;
  switch(what1)
  {
  case _POSITION_RIGHT:   
    set_position_right(anim1_head, anim1_left_arm, anim1_left_leg, anim1_right_arm, anim1_right_leg); 
    break;
  case _POSITION_STAND:   
    set_position_stand(anim1_head, anim1_left_arm, anim1_left_leg, anim1_right_arm, anim1_right_leg); 
    break;    
  case _POSITION_WAITFOR:   
    set_position_waitfor(anim1_head, anim1_left_arm, anim1_left_leg, anim1_right_arm, anim1_right_leg); 
    break;
  case _POSITION_HELLO1:   
    set_position_hello1(anim1_head, anim1_left_arm, anim1_left_leg, anim1_right_arm, anim1_right_leg); 
    break;
  case _POSITION_HELLO2:   
    set_position_hello2(anim1_head, anim1_left_arm, anim1_left_leg, anim1_right_arm, anim1_right_leg); 
    break;
  case _POSITION_PUMP0:   
    set_position_pump_high(anim1_head, anim1_left_arm, anim1_left_leg, anim1_right_arm, anim1_right_leg); 
    break;
  case _POSITION_PUMP1:   
    set_position_pump_low(anim1_head, anim1_left_arm, anim1_left_leg, anim1_right_arm, anim1_right_leg); 
    break;
  case _POSITION_SEMIPUMP0:   
    set_position_semipump_high(anim1_head, anim1_left_arm, anim1_left_leg, anim1_right_arm, anim1_right_leg); 
    break;
  case _POSITION_SEMIPUMP1:   
    set_position_semipump_low(anim1_head, anim1_left_arm, anim1_left_leg, anim1_right_arm, anim1_right_leg); 
    break;
  case _POSITION_PLANE:   
    set_position_plane(anim1_head, anim1_left_arm, anim1_left_leg, anim1_right_arm, anim1_right_leg); 
    break;
  case _POSITION_HULK:   
    set_position_hulk(anim1_head, anim1_left_arm, anim1_left_leg, anim1_right_arm, anim1_right_leg); 
    break;
  default:
  case _POSITION_CURRENT:
    for(int i=0; i<N_HEAD; i++)
    {
      anim1_head[i] = pos_head[i];
    }
    for(int i=0; i<N_ARM; i++)
    {
      anim1_left_arm[i] = pos_left_arm[i];
      anim1_right_arm[i] = pos_right_arm[i];
    }
    for(int i=0; i<N_LEG; i++)
    {
      anim1_left_leg[i] = pos_left_leg[i];
      anim1_right_leg[i] = pos_right_leg[i];
    }
    break;
  }
  switch(what2)
  {
  case _POSITION_RIGHT:   
    set_position_right(anim2_head, anim2_left_arm, anim2_left_leg, anim2_right_arm, anim2_right_leg); 
    break;
  case _POSITION_STAND:   
    set_position_stand(anim2_head, anim2_left_arm, anim2_left_leg, anim2_right_arm, anim2_right_leg); 
    break;    
  case _POSITION_WAITFOR:   
    set_position_waitfor(anim2_head, anim2_left_arm, anim2_left_leg, anim2_right_arm, anim2_right_leg); 
    break;
  case _POSITION_HELLO1:   
    set_position_hello1(anim2_head, anim2_left_arm, anim2_left_leg, anim2_right_arm, anim2_right_leg); 
    break;
  case _POSITION_HELLO2:   
    set_position_hello2(anim2_head, anim2_left_arm, anim2_left_leg, anim2_right_arm, anim2_right_leg); 
    break;
  case _POSITION_PUMP0:   
    set_position_pump_high(anim2_head, anim2_left_arm, anim2_left_leg, anim2_right_arm, anim2_right_leg); 
    break;
  case _POSITION_PUMP1:   
    set_position_pump_low(anim2_head, anim2_left_arm, anim2_left_leg, anim2_right_arm, anim2_right_leg); 
    break;
  case _POSITION_SEMIPUMP0:   
    set_position_semipump_high(anim2_head, anim2_left_arm, anim2_left_leg, anim2_right_arm, anim2_right_leg); 
    break;
  case _POSITION_SEMIPUMP1:   
    set_position_semipump_low(anim2_head, anim2_left_arm, anim2_left_leg, anim2_right_arm, anim2_right_leg); 
    break;
  case _POSITION_PLANE:   
    set_position_plane(anim2_head, anim2_left_arm, anim2_left_leg, anim2_right_arm, anim2_right_leg); 
    break;
  case _POSITION_HULK:   
    set_position_waitfor(anim2_head, anim2_left_arm, anim2_left_leg, anim2_right_arm, anim2_right_leg); 
    break;
  default:
  case _POSITION_CURRENT:
    for(int i=0; i<N_HEAD; i++)
    {
      anim2_head[i] = pos_head[i];
    }
    for(int i=0; i<N_ARM; i++)
    {
      anim2_left_arm[i] = pos_left_arm[i];
      anim2_right_arm[i] = pos_right_arm[i];
    }
    for(int i=0; i<N_LEG; i++)
    {
      anim2_left_leg[i] = pos_left_leg[i];
      anim2_right_leg[i] = pos_right_leg[i];
    }
    break;
  }
  do_anim(time);
}


